/*****************************************************************************
 * $Workfile:: control.c                                                     $
 * $Revision:: 4                                                             $
 * $JustDate::  5/20/10                                                      $
 *
 *      GENIE V ECU
 *      control algorithm
 *****************************************************************************/

#include "main.h"
#include "alarm.h"
#include "led.h"
#include "battery.h"
#include "control.h"
#include "curve.h"
#include "display.h"
#include "faults.h"
#include "fet.h"
#include "joystick.h"
#include "nvm.h"
#include "serial.h"
#include "switches.h"


#define CONTROL_INTERVAL_MS(x)    ((x)/CONTROL_CYCLE)

#define PWM_PERIOD  ((UINT16)255)


//#define CHASSIS_UP_PWM   lift_setup.bChassisLiftUpPwm
#define CHASSIS_UP_PWM   lift_setup.lift_up_curve.bPwmPosAdjust

#define MOTOR_ON_DELAY   LIFT_Data.bMotorEnableDelay

#define TILT_BRAKE_ON_DELAY   LIFT_Data.bTiltBrakeDelay

#define LEVEL_BRAKE_APPLY_DELAY   LIFT_Data.bLevelBrakeDelay

#define TURN_BOOST       lift_setup.bSteeringBoostPwm

#define MODE_CHANGE_TIMER           CONTROL_INTERVAL_MS(200)

#define DRIVE_DIRECTION_SWITCH_DELAY LIFT_Data.wDirectionDelay

#define NEUTRAL_BRAKE_APPLY_DELAY  LIFT_Data.wBrakeDelay

#define NEUTRAL_WHEEL_PWM          lift_setup.bNeutralSteeringPwm
#define NEUTRAL_WHEEL_PWM_OFF_DELAY LIFT_Data.wSteeringOffDelay

#define BRAKE_DETECTION_TIME     CONTROL_INTERVAL_MS(15000)
#define BRAKE_DETECTION_TIME_2      CONTROL_INTERVAL_MS(02000)
#define BRAKE_DETECTION_TIME_3      CONTROL_INTERVAL_MS(05000)
#define LOAD_TIME    99

#define PLATFORM_UP_PWM lift_setup.bPlatformLiftUpMaxPwm
#define OVERLOAD_STABILIZATION_TIME LIFT_Data.wOverloadStabilizationPeriod
#define UP_FET_SPIKE_TIME  CONTROL_INTERVAL_MS(2000)
#define FET_DISABLE_SPIKE_TIME  CONTROL_INTERVAL_MS(10000)

//#define OVERLOAD_CUTOUT_OPTION (lift_options.fOverload2_cutout || (lift_options.fOverload1_cutout && lift_options.fAngle_option))  // disabled
#define OVERLOAD_CUTOUT_OPTION lift_options.fOverload2_cutout
#define HEIGHT_OFFSET   70
// #define OVERLOAD_CUTOUT_OPTION                 0    // enabled

#define DESCENT_DELAY_OPTION      lift_options.fDescent_delay              // enabled

// #ifdef LLDC_ALWAYS_ON
// lift drive cutout is enabled all the time.
#define LIFT_DRIVE_CUTOUT_OPTION  1
// #else
//    #define LIFT_DRIVE_CUTOUT_OPTION  lift_options.fLevel_lift_drive_cutout     // enabled
// #endif

#define MOTOR_CONTROL_INPUT1            u8GetStatus5C()//IO.PDR7.BIT.B6      // Motor control LED input to ECU
#define MOTOR_CONTROL_INPUT2            u8GetStatus5B()//IO.PDR8.BIT.B4      // Motor control LED input to ECU
#define MOTOR_LED_MAX_ON_TIME           CONTROL_INTERVAL_MS(250)//500
#define MOTOR_LED_MIN_ON_TIME           CONTROL_INTERVAL_MS(50)
#define MOTOR_LED_FAULT_OFF_TIME        CONTROL_INTERVAL_MS(6000)
#define MOTOR_LED_WORD_OFF_TIME         CONTROL_INTERVAL_MS(2000)
#define MOTOR_LED_NIBBLE_OFF_TIME       CONTROL_INTERVAL_MS(400)//600
#define MOTOR_LED_MIN_OFF_TIME          CONTROL_INTERVAL_MS(50)
#define MOTOR_LED_FAULT1_MAX_ON_TIME    CONTROL_INTERVAL_MS(300)
#define MOTOR_LED_FAULT1_MIN_ON_TIME    CONTROL_INTERVAL_MS(200)
#define MOTOR_LED_FAULT2_MAX_ON_TIME    CONTROL_INTERVAL_MS(600)
#define MOTOR_LED_FAULT2_MIN_ON_TIME    CONTROL_INTERVAL_MS(400)
#define MOTOR_LED_FAULT3_MAX_ON_TIME    CONTROL_INTERVAL_MS(1200)
#define MOTOR_LED_FAULT3_MIN_ON_TIME    CONTROL_INTERVAL_MS(800)

/*
#define FORCE_NEUTRAL()  \
{   \
    if ( LIFT_Data.nRampedCounts1 >= 1)   \
    {   \
        LIFT_Data.nRampedCounts1_X_1000 = 1000l;  \
    }   \
    else if ( LIFT_Data.nRampedCounts1 <= -1) \
    { \
        LIFT_Data.nRampedCounts1_X_1000 = -1000l;  \
    } \
    if ( LIFT_Data.nRampedCounts2 >= 1)   \
    {   \
        LIFT_Data.nRampedCounts2_X_1000 = 1000l;  \
    }   \
    else if ( LIFT_Data.nRampedCounts2 <= -1) \
    { \
        LIFT_Data.nRampedCounts2_X_1000 = -1000l;  \
    } \
    LIFT_Data.calc_pwm1 = 0;  \
    LIFT_Data.calc_pwm2 = 0;  \
}
*/

struct LIFT_STRUCT LIFT_Data;
//static struct
//{
//   UINT8 fStopNon1:1;
//   UINT8 fStopNon2:1;
//}PWM_Local;

#define NUMBER_OF_TABLE_POINTS  6
/*
const INT16 atanTable[] =
{
    0, 71, 146, 229, 327, 450,
   0, 90, 180, 270, 360, 450
};
*/


static void DRIVE_PwmAndFet1(void);
//static void DRIVE_PwmAndFet2( void );
//static void InitTimerW( void );
void LIFT_Pwm(void);
static void ramp_up1(void);
//static void ramp_up2( void );
static INT8 GET_target_counts(void);
static void LIFT_UP_ON(void);
static UINT8 Zapi_Brake_Detect(void);

//static INT8 GET_steer_counts( void );
//INT16 atan2Approx( INT16, INT16 );
//INT16 GET_magnitude( INT16, INT16 );

UINT8 MotorLED1;
UINT8 MotorFaultBuild1;
//UINT8 MotorLED2;
//UINT8 MotorFaultBuild2;
static UINT8  DebounceMCESleep = 15;
static INT16 wCurrentPercentage;
//static unsigned int sui_UpFet_Counter = 0;
static INT16 wTestTriggerPoint;

#define PCU_NO_ACTION_DELAY  1000L   // PCU no action delay, 1000 * 10ms = 10s
UINT16 u16PcuNoActionCnt = PCU_NO_ACTION_DELAY;

/*******************************************************************************
 *
 * PROCEDURE:  CONTROL_Setup()
 *
 * DESCRIPTION: Copies adjusted nvm data to LIFT_Data struct.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * LIFT_Data        R/W owner
 * lift_setup      R   nvm data
 ******************************************************************************/

void
CONTROL_Setup(void)
{
   //
   // the NVM parameters must be converted to the number of the CONTROL_CYCLES.
   //

   LIFT_Data.wBrakeDelay  = lift_setup.bBrakeDelay
                            * (BRAKE_APPLY_DELAY_TO_COUNT / CONTROL_CYCLE);

   LIFT_Data.wSteeringOffDelay = lift_setup.bSteeringOffDelay
                                 * (NEUTRAL_ZONE_STEERING_OFF_DELAY_TO_COUNT / CONTROL_CYCLE);

   LIFT_Data.wOverloadStabilizationPeriod = lift_setup.bOverloadStabilizationPeriod
         * (OVERLOAD_STABILIZATION_PERIOD_TO_COUNT / CONTROL_CYCLE);

   LIFT_Data.wDirectionDelay = lift_setup.bDirectionDelay
                               * (DRIVE_DIRECTION_SWITCH_DELAY_TO_COUNT / CONTROL_CYCLE);

   LIFT_Data.bMotorEnableDelay = lift_setup.bMotorEnableDelay
                                 * (MOTOR_ENABLE_DELAY_TO_COUNT / CONTROL_CYCLE);

   LIFT_Data.bTiltBrakeDelay = lift_setup.bTiltBrakeDelay
                               * (TILT_BRAKE_DELAY_TO_COUNT / CONTROL_CYCLE);

   LIFT_Data.bLevelBrakeDelay = lift_setup.bLevelBrakeDelay
                                * (LEVEL_BRAKE_DELAY_TO_COUNT / CONTROL_CYCLE);

}

void
FORCE_NEUTRAL(void)
{
   if(LIFT_Data.nRampedCounts1 >= 1)
   {
      LIFT_Data.nRampedCounts1_X_1000 = 1000l;
   }
   else if(LIFT_Data.nRampedCounts1 <= -1)
   {
      LIFT_Data.nRampedCounts1_X_1000 = -1000l;
   }
   if(LIFT_Data.nRampedCounts2 >= 1)
   {
      LIFT_Data.nRampedCounts2_X_1000 = 1000l;
   }
   else if(LIFT_Data.nRampedCounts2 <= -1)
   {
      LIFT_Data.nRampedCounts2_X_1000 = -1000l;
   }
   LIFT_Data.calc_pwm1 = 0;
   LIFT_Data.calc_pwm2 = 0;
}


/*******************************************************************************
 *
 * PROCEDURE:  ALGOR_Overload( int n_Pressure_Percent_of_Maximum ALGOR_Overload )
 *
 * DESCRIPTION:  Detects lift overload conditions if overload option is set.
 *               the false overload conditions while lifting up or down
 *               is ignored. The function called once per lift control cycle.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * LIFT_Data        R/W owner
 * lift_setup       R   nvm data
 ******************************************************************************/

void
ALGOR_Overload(int n_Load_Sensing_Pressure_Percent_of_Maximum)
{
   static unsigned int  sui_Overload_Reset_Counter;
   static unsigned int  sui_Overload_Disable_Counter = 0;      // Counts After a Move  before checking OL
   static unsigned int  sui_UpFet_Counter = 0;              // Counts From begining of Move 2 Secs
   static unsigned char scu_UpFetSet;                    // TRUE FALSE Moving not moving
   static unsigned char scu_80counter, scu_90counter, scu_99counter; // Counter for display values
   // static unsigned long slu_SaveTime;
   static unsigned int  sui_BleedTime = BLEED_TIME;
   int  n_PressureBleedComp;
   long Templ;

   wTestTriggerPoint = OVERLOAD_FAULT_TRIP_LEVEL;
   // ET_WORKING <AMWP_PASS_FAIL>
   //#define COMPILE_THIS  1
   //#ifdef COMPILE_THIS
   if(!OVERLOAD_CUTOUT_OPTION || n_Load_Sensing_Pressure_Percent_of_Maximum < 0)
      LIFT_Data.nOverloadPercent = 0;

   if(OVERLOAD_CUTOUT_OPTION && NVM_CheckSetupInit()   //&& !LIFT_Data.wDownSafetyTimerStowed
      && (((MAIN_Data.fAnglePolarity && (MAIN_Data.wMeasured_Scissor_Angle > MAIN_Data.wMinimum_Angle))
           || (!MAIN_Data.fAnglePolarity && (MAIN_Data.wMeasured_Scissor_Angle < MAIN_Data.wMinimum_Angle))))
      && MAIN_Data.fUpLimit
      && !(LIFT_Data.hd_fet & (/*FET_UP |*/ FET_FORWARD | FET_REVERSE
            | FET_RIGHT | FET_LEFT | FET_DOWN)))   // If not moving up nor down, you can move up, then stop, and cannot get back down again!!
   {
      // **************
      //  if moving up
      // **************
      if(LIFT_Data.hd_fet & FET_UP)
      {
         sui_BleedTime = BLEED_TIME;
         n_PressureBleedComp = 0;
         sui_Overload_Disable_Counter = FET_DISABLE_SPIKE_TIME;
         if(sui_UpFet_Counter)
            sui_UpFet_Counter--;
         if(!scu_UpFetSet && !sui_UpFet_Counter)
         {
            sui_UpFet_Counter = UP_FET_SPIKE_TIME;
         }
         MAIN_Data.fUpSpike = 0;
         if(sui_UpFet_Counter)
         {
            MAIN_Data.fUpSpike = 1;
            n_Load_Sensing_Pressure_Percent_of_Maximum = wTestTriggerPoint - OVERLOAD_FAULT_HYSTERESIS + 1;
         }
         else if((n_Load_Sensing_Pressure_Percent_of_Maximum > (100 + OVERLOAD_UP_OFFSET))
                 /*&& MAIN_Data.fPressureValue*/)
         {
            n_Load_Sensing_Pressure_Percent_of_Maximum -= OVERLOAD_UP_OFFSET;
            LIFT_Data.nOverloadPercent = n_Load_Sensing_Pressure_Percent_of_Maximum;
         }
         else if(n_Load_Sensing_Pressure_Percent_of_Maximum > 0/*79*/)
         {
            LIFT_Data.nOverloadPercent = n_Load_Sensing_Pressure_Percent_of_Maximum;
         }
         scu_UpFetSet = 1;
      }
      // **************
      //  if stationary
      // **************
      else
      {
         if(scu_UpFetSet && !sui_UpFet_Counter)
         {
            sui_UpFet_Counter = UP_FET_SPIKE_TIME;
         }
         if(sui_UpFet_Counter)
            sui_UpFet_Counter--;
         else if(!sui_Overload_Disable_Counter && sui_BleedTime)
            sui_BleedTime--;
         if((/*(ELEC2 == lift_setup.bLiftType) ||*/ (ELEC3 == lift_setup.bLiftType))
           )
         {
            // 0807/HD
            Templ = (long)(BLEED_TIME - sui_BleedTime) * (long)(BLEED_COMP);
            Templ = Templ * (long)(MAIN_Data.wHeightOffset);
            Templ = Templ / ((long)(BLEED_TIME) * (long)(HEIGHT_OFFSET));
            n_PressureBleedComp = (int)(Templ);
         }
         else
         {
            n_PressureBleedComp = 0;
         }
         if(!sui_UpFet_Counter)
            LIFT_Data.nOverloadPercent = n_Load_Sensing_Pressure_Percent_of_Maximum + n_PressureBleedComp;

         scu_UpFetSet = 0;
      }
      // **************
      // OL Test
      // **************


      wCurrentPercentage = n_Load_Sensing_Pressure_Percent_of_Maximum + n_PressureBleedComp;
      //    GTFtemp = wCurrentPercentage;
      if((wCurrentPercentage) >= wTestTriggerPoint)
      {
         sui_Overload_Reset_Counter = 0;
         if(sui_Overload_Disable_Counter && !(LIFT_Data.hd_fet & FET_UP))
         {
            sui_Overload_Disable_Counter--;
         }
         else
         {
            LIFT_Data.wOverloadCounter++;
            if(LIFT_Data.wOverloadCounter > OVERLOAD_STABILIZATION_TIME)
            {
               LIFT_Data.fOverload = TRUE;      // ? Is movement locked out until reset?
               LIFT_Data.wOverloadCounter = OVERLOAD_STABILIZATION_TIME;
            }
         }
      }
      else
      {
         if(sui_Overload_Disable_Counter)
         {
            sui_Overload_Disable_Counter--;
         }
         //       else
         //       {
         //          slu_SaveTime = MAIN_Data.dwElevationTime;
         //       }
         LIFT_Data.wOverloadCounter = 0;

         wCurrentPercentage = n_Load_Sensing_Pressure_Percent_of_Maximum + n_PressureBleedComp;
         if((wCurrentPercentage) <= (wTestTriggerPoint - OVERLOAD_FAULT_HYSTERESIS))   // This is hysteresis for resetting on-the-fly.
         {
            sui_Overload_Reset_Counter += 4;
            if(sui_Overload_Reset_Counter > OVERLOAD_STABILIZATION_TIME)
            {
               LIFT_Data.fOverload = FALSE;    // The weight has been removed.  Allow Movement again.
               sui_Overload_Reset_Counter = OVERLOAD_STABILIZATION_TIME;
            }
         }
         else
         {
            sui_Overload_Reset_Counter = 0;
         }
      }
   }
   else if(LIFT_Data.hd_fet & FET_DOWN)
   {
      sui_UpFet_Counter = UP_FET_SPIKE_TIME;
      sui_BleedTime = BLEED_TIME;
      sui_Overload_Disable_Counter = FET_DISABLE_SPIKE_TIME;
   }
   if(!(LIFT_Data.hd_fet & (FET_FORWARD | FET_REVERSE
                            | FET_RIGHT | FET_LEFT | FET_DOWN)))
   {  //    Check flags for load display
      if(LIFT_Data.nOverloadPercent > 108)
      {
         if (scu_99counter > LOAD_TIME)      LIFT_Data.fLoad99 = TRUE;
         else scu_99counter++;
      }
      else
      {
         if (!scu_99counter )    LIFT_Data.fLoad99 = FALSE;
         else scu_99counter--;
      }
      if(LIFT_Data.nOverloadPercent > 98)
      {
         if (scu_90counter > LOAD_TIME)      LIFT_Data.fLoad90 = TRUE;
         else scu_90counter++;
      }
      else
      {
         if (!scu_90counter )    LIFT_Data.fLoad90 = FALSE;
         else scu_90counter--;
      }
      if(LIFT_Data.nOverloadPercent > 87)
      {
         if (scu_80counter > LOAD_TIME)      LIFT_Data.fLoad80 = TRUE;
         else scu_80counter++;
      }
      else
      {
         if (!scu_80counter )    LIFT_Data.fLoad80 = FALSE;
         else scu_80counter--;
      }
   }
   //#endif
}


/*******************************************************************************
 *
 * PROCEDURE:   MotorFaultMonitor()
 *
 * DESCRIPTION: The main control procedure. It is invoked once per control cycle.
 *              to calculate motor and coils control values.
 *              The actual vlues intitated by main loop.
 *              The control inputs are acepted from chassis switches or platform
 *              controller. The chassis if activated has high priority than platform.
 *
 * VARIABLE NAME    R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * LIFT_Data       R/W owner
 * lift_setup      R   nvm data
 ******************************************************************************/

void
MotorFaultMonitor(void)
{
   // We monitor the motor controller's LED output to capture the LED pulses that
   // indicate the current alarm status of the motor controller.  The values are
   // sent via a series of pulse trains, separated by a delay larger than the
   // time for three pulses, and the two digit code is separated by very large delay.

   // Notes:
   // * Since a '0' digit is the absence of pulses, we cannot detect single digit codes.
   //   All codes need to be two digits and have values > 0x10.
   // * The ZAPI controllers use a single pulse as a heart beat, their documentation says
   //   they send a 0x01, but how we construct the value, this gets read as a 0x10 value.

   static UINT8 LastFault = 0xFF;  // If LastFault == 0xFF, it is the first error code received from the Zapi.
                                   // Zapi cannot send us a 0xFF error code.
   if(NVM_CheckSetupInit())
   {
      MotorLED1 <<= 1;
      if(MOTOR_CONTROL_INPUT1)
         MotorLED1 |= 0x01;

      //        MotorLED2 <<= 1;
      //        if ( MOTOR_CONTROL_INPUT2 )
      //            (MotorLED2 |= 0x01);

      LIFT_Data.wMotorLEDCounter1++;

      // Only change the current signal state if we have 4 consecutive values that are the same.
      if((MotorLED1 & 0x0f) == 0x00)
      {
         if(!LIFT_Data.fMotorLED1)
         {
            LIFT_Data.fMotorLED1 = 1;
            LIFT_Data.wMotorLEDCounter1 = 0;
            LIFT_Data.fMotorValidBit1 = 0;
         }
      }
      else if((MotorLED1 & 0x0f) == 0x0f)
      {
         if(LIFT_Data.fMotorLED1)
         {
            LIFT_Data.fMotorLED1 = 0;
            LIFT_Data.wMotorLEDCounter1 = 0;
            LIFT_Data.fMotorValidBit1 = 0;
         }
      }

      // Process the current incoming pulse train state value.
      if(LIFT_Data.fMotorLED1)
      {
         if(LIFT_Data.wMotorLEDCounter1 > MOTOR_LED_MAX_ON_TIME)
         {
            MotorFaultBuild1 = 0x00;
         }
         else if(LIFT_Data.wMotorLEDCounter1 > MOTOR_LED_MIN_ON_TIME)
         {
            if(LIFT_Data.fMotorNibble1 && !LIFT_Data.fMotorValidBit1)
               MotorFaultBuild1 += 0x01;
            else if(!LIFT_Data.fMotorValidBit1)
               MotorFaultBuild1 += 0x10;
            LIFT_Data.fMotorValidBit1 = 1;
         }
      }
      else
      {
         if(LIFT_Data.wMotorLEDCounter1 > MOTOR_LED_FAULT_OFF_TIME)
         {
            LIFT_Data.bMotorFault1 = 0x00;
            LastFault = 0x00;
         }
         if(LIFT_Data.wMotorLEDCounter1 > MOTOR_LED_WORD_OFF_TIME)
         {
            // Is this the first error pulse train we are reading in?
            if(LastFault == 0xFF)
            {
               // Throw it away, it may be incomplete.
               LastFault = 0;
               MotorFaultBuild1 = 0;
            }

            if(LIFT_Data.fMotorNibble1)
            {
               LastFault = LIFT_Data.bMotorFault1;
               LIFT_Data.bMotorFault1 = MotorFaultBuild1;
            }
            MotorFaultBuild1 = 0;
            LIFT_Data.fMotorNibble1 = 0;

            /*if (ELEC2 == lift_setup.bLiftType)
            {
                switch ( LIFT_Data.bMotorFault1 )
                {
                    default:
                        break;
                    case 0x11:
                        NVM_SaveFault(FAULT_MC_CURRENT_SENSOR);
                        break;
                    case 0x12:
                        NVM_SaveFault(FAULT_HARDWARE_FAILSAFE);
                        break;
                    case 0x13:
                        NVM_SaveFault(FAULT_MOTOR_SHORT);
                        break;
                    case 0x21:
                        NVM_SaveFault(FAULT_MC_SRO);
                        break;
                    case 0x23:
                    case 0x33:
                        NVM_SaveFault(FAULT_MC_THROTTLE);
                        break;
                    case 0x31:
                        NVM_SaveFault(FAULT_EMER_REVERSE);
                        break;
                    case 0x22:
                        NVM_SaveFault(FAULT_HPD);
                        break;
            //                        case 0x31:
                    case 0x32:
            //                        case 0x34:
            //                            NVM_SaveFault(FAULT_MAIN_CONTACTOR);
                        NVM_SaveFault(FAULT_BRAKE_COIL);
                        LIFT_Data.fMotorFault = 1;
                        break;
                    case 0x41:
                        NVM_SaveFault(FAULT_LOW_VOLTAGE);
                        break;
                    case 0x42:
                        NVM_SaveFault(FAULT_OVER_VOLTAGE);
                        break;
                    case 0x43:
                        NVM_SaveFault(FAULT_THERMAL_CUTBACK);
                        break;
            //                        case 0x44:
            //                            NVM_SaveFault(FAULT_OVERHEATED_MOTOR);
            //                            break;
                }
            }
            else if ( (ELEC1 == lift_setup.bLiftType) && !lift_options.fMotorCtlr_option )
            {
                switch ( LIFT_Data.bMotorFault1 )
                {
                    default:
                        break;
                    case 0x11:
                        NVM_SaveFault(FAULT_MC_CURRENT_SENSOR);
                        break;
                    case 0x12:
                        NVM_SaveFault(FAULT_HARDWARE_FAILSAFE);
                        break;
                    case 0x13:
                        NVM_SaveFault(FAULT_MOTOR_SHORT);
                        break;
                    case 0x14:
                        NVM_SaveFault(FAULT_MC_SRO);
                        break;
                    case 0x21:
                    case 0x24:
                        NVM_SaveFault(FAULT_MC_THROTTLE);
                        break;
                    case 0x22:
                        NVM_SaveFault(FAULT_EMER_REVERSE);
                        break;
                    case 0x23:
                        NVM_SaveFault(FAULT_HPD);
                        break;
                    case 0x31:
                    case 0x32:
                    case 0x33:
                    case 0x34:
                        NVM_SaveFault(FAULT_MAIN_CONTACTOR);
                        LIFT_Data.fMotorFault = 1;
                        break;
                    case 0x41:
                        NVM_SaveFault(FAULT_LOW_VOLTAGE);
                        break;
                    case 0x42:
                        NVM_SaveFault(FAULT_OVER_VOLTAGE);
                        break;
                    case 0x43:
                        NVM_SaveFault(FAULT_THERMAL_CUTBACK);
                        break;
                    case 0x44:
                        NVM_SaveFault(FAULT_OVERHEATED_MOTOR);
                        break;
                }
            }
            else*/ if(ELEC1 == lift_setup.bLiftType)
            {
               // Large Electric Lift with a ZAPI TriPlat motor controller.
               switch(LIFT_Data.bMotorFault1)
               {
                  case 0x11:
                     LIFT_Data.fMotorController = 0;
                     break;
                  // Errors we cannot detect, single digit error codes cannot
                  //  be read via the LED signalling.
                  //break;
                  case 0x41: // FORW + BACK
                     NVM_SaveMotorCtlrFault(FAULT_MC_SRO);
                     break;
                  case 0x34: // Serial Error #1
                  case 0x91: // Serial Error #1
                  case 0x31: // No CAN Msg
                  case 0x85: // No CAN Msg
                     break;
                  case 0x46: // Waiting for Node
                     break;
                  case 0x35: // EEPROM KO
                  case 0x32: // Flash Checksum
                  case 0x86: // Flash Checksum
                  case 0x33: // Wrong RAM Memory
                  case 0x87: // Wrong RAM Memory

                  case 0x23: // Mismatch N.

                  case 0x25: // AUX Enable Short
                  case 0x15: // AUX Drv Shorted
                  case 0x42: // Driver 1 KO
                  case 0x93: // Driver 1 KO
                  case 0x84: // Driver 1 KO
                  case 0x43: // Driver 2 KO
                  case 0x71: // Driver 2 KO
                  case 0x79: // Driver 3 KO
                     NVM_SaveMotorCtlrFault(FAULT_MOTOR_CTLR);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x36: // Logic Failure #1
                  case 0x37: // Battery Low
                  case 0x51: // Logic Failure #1
                  case 0x28: // Wrong Set Bat.
                     NVM_SaveMotorCtlrFault(FAULT_LOW_VOLTAGE);
                     break;
                  case 0x38: // Driver Shorted
                  case 0x39: // Contactor Driver
                  case 0x26: // MC Enable Short
                  case 0x27: // MC Coil Open
                  case 0x92: // Contactor Closed
                  case 0x61: // Contactor Open
                     NVM_SaveMotorCtlrFault(FAULT_MAIN_CONTACTOR);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x44: // EB1 Drv Shorted
                  case 0x13: // EB1 Coil Shorted
                  case 0x17: // EB 1 Coil Open

                  case 0x12: // EB2 Drv Shorted
                  case 0x14: // EB2 Coil Shorted
                  case 0x18: // EB 2 Coil Open

                  case 0x16: // EB Coil Shorted
                  case 0x45: // Brake Enable
                     NVM_SaveMotorCtlrFault(FAULT_RIGHT_BRAKE_COIL);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x21: // Steer Out Of Range
                  case 0x82: // Steer Out Of Range
                     NVM_SaveMotorCtlrFault(FAULT_STEERING_INPUT_RANGE);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x29: // Analog Input
                  case 0x83: // Analog Input
                     //if (LastFault == LIFT_Data.bMotorFault1)
                     NVM_SaveMotorCtlrFault(FAULT_HARDWARE_FAILSAFE);
                     break;
                  case 0x19: // Output Mismatch
                  case 0x52: // Left VMN Low
                  case 0x63: // Left Stby I High
                     NVM_SaveMotorCtlrFault(FAULT_LEFT_MOTOR);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x53: // Left VMN High
                  case 0x74: // ArmSx coil Short   //vince
                     NVM_SaveMotorCtlrFault(FAULT_LEFT_MOTOR_SHORT);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x24: // A12 Drv Fail
                  case 0x54: // Right VMN Low
                  case 0x64: // Right Stby I High
                     NVM_SaveMotorCtlrFault(FAULT_RIGHT_MOTOR);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x55: // Right VMN High
                  case 0x73: // ArmDx Coil Short               // vince
                     NVM_SaveMotorCtlrFault(FAULT_RIGHT_MOTOR_SHORT);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x56: // Pump VMN Low
                  case 0x62: // Pump |= 0 Ever
                  case 0x65: // Pump Stby I High
                  case 0x72: // Pump I Sens. KO
                  // Temporarily makes these a pump fault.
                  case 0x58: // VMN Low
                  case 0x59: // VMN High
                     NVM_SaveMotorCtlrFault(FAULT_PUMP_MOTOR);
                     break;
                  case 0x57: // Pump VMN High
                     NVM_SaveMotorCtlrFault(FAULT_PUMP_MOTOR_SHORT);
                     break;
                  case 0x66: // High Field Cur
                  case 0x67: // No Field Cur
                  case 0x68: // Capacitor Charge
                     NVM_SaveMotorCtlrFault(FAULT_MC_CURRENT_SENSOR);
                     break;
                  case 0x69: // High Temperature
                     NVM_SaveMotorCtlrFault(FAULT_THERMAL_CUTBACK);
                     break;
                  case 0x76: // Th. Sensor KO
                  case 0x77: // Trac. Sens. KO
                  case 0x78: // Wait for Enable
                  case 0x94: // Trac. En. Short
                  case 0x81: // Pump En. Short
                     NVM_SaveMotorCtlrFault(FAULT_HPD);
                     break;
                  case 0x88: // FldDx Coil Short
                     NVM_SaveMotorCtlrFault(FAULT_LEFT_MOTOR_FIELD_SHORT);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x89: // FldSx Coil Short
                     NVM_SaveMotorCtlrFault(FAULT_RIGHT_MOTOR_FIELD_SHORT);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x75:  // V Field Not OK           //vince
                     NVM_SaveMotorCtlrFault(FAULT_MOTOT_FIELD_OPEN);
                     LIFT_Data.fMotorFault = 1;                        //vince
                     break;
                  case 0x22: // Incorrect Start
                  // This error is thrown when the ZAPI controller is connected to
                  // its PC configuration software, so its not really an error.
                  default:
                     break;
               }
            }
            else if(ELEC3 == lift_setup.bLiftType)   // 0807 with ZAPI motor controller
            {
               switch(LIFT_Data.bMotorFault1)
               {
                  default:
                     break;
                  case 0x42:
                  case 0x44:
                  case 0x68:
                  case 0x76:
                  case 0x86:
                     NVM_SaveFault(FAULT_RIGHT_BRAKE_COIL);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x17:
//                   case 0x63:
                  case 0x88:
                  case 0x69:
                  case 0x92:
                     if(LastFault == LIFT_Data.bMotorFault1)
                        NVM_SaveFault(FAULT_MC_CURRENT_SENSOR);
                     break;
                  case 0x48:
                  case 0x57:
//                   NVM_SaveFault(FAULT_LEFT_BRAKE_COIL);
//                   LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x08:
                  case 0x12:
                  case 0x55:
                  case 0x80:
                  case 0x96:
                     if(LastFault == LIFT_Data.bMotorFault1)
                        NVM_SaveFault(FAULT_HARDWARE_FAILSAFE);
                     break;
                  case 0x81:
                     if(LastFault == LIFT_Data.bMotorFault1)
                        NVM_SaveFault(FAULT_MC_SRO);
                     break;
                  case 0x78:
                  case 0x85: // VACC Out Range
                  case 0x91:
//                      NVM_SaveFault(FAULT_MC_THROTTLE);
//                      break;
                  case 0x64:
//                   case 0x76:
                  case 0x79:
                     if(LastFault == LIFT_Data.bMotorFault1)
                        NVM_SaveFault(FAULT_HPD);
                     break;
                  case 0x74:
                  case 0x75:
                  case 0x77:
                     NVM_SaveFault(FAULT_MAIN_CONTACTOR);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x41:
                     // Error 0x41 is only thrown at start-up, stop monitoring after waiting
                     //  long enough see at least one pulse train from the Zapi.
                     if(TIMER_Data.wSystemTime < 18000)
                     {
                        NVM_SaveFault(FAULT_LOW_VOLTAGE);
                     }
                     break;
                  case 0x54:
                  case 0x98:
                     NVM_SaveFault(FAULT_LOW_VOLTAGE);
                     break;
                  case 0x61:
                  case 0x62:
                  case 0x65:
                  case 0x67:
                     NVM_SaveFault(FAULT_THERMAL_CUTBACK);
                     break;
                  case 0x11:
                     if(LastFault == LIFT_Data.bMotorFault1)
                        NVM_SaveFault(FAULT_OVERHEATED_MOTOR);
                     break;
                  case 0x63:
                     if(!LIFT_Data.fMotorController)
                     {
                        LIFT_Data.fMotorController = 1;
                     }
                  case 0x32:    // VMN Not OK Left
                     //                            case 0x36:
                     //                            case 0x38:
                     if(LIFT_Data.fMotorController)
                     {
                        NVM_SaveFault(FAULT_LEFT_MOTOR);
                     }
                     else
                     {
                        NVM_SaveFault(FAULT_LEFT_MOTOR_SHORT);
                     }
                     LIFT_Data.fMotorFault = 1;
                     break;
//                        case 0x38: <D6>
//                        case 0x37: <D8>
                  case 0x38:
                     NVM_SaveFault(FAULT_LEFT_MOTOR);
                     LIFT_Data.fMotorFault = 1;
                     break;

//                        case 0x34:
//                        case 0x35:
//                        case 0x37:  <D6>
//                        case 0x38:  <D8>
                  case 0x37:
                     NVM_SaveFault(FAULT_RIGHT_MOTOR);
                     break;
                  case 0x53:
                     NVM_SaveFault(FAULT_HIGH_NEUTRAL_CURRENT);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x84:
                     NVM_SaveFault(FAULT_STEERING_INPUT_RANGE);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x33:
                     if(TIMER_Data.wSystemTime < 20000)
                        break;
                  case 0x52:
                  case 0x56:
                  case 0x73:
                     NVM_SaveFault(FAULT_PUMP_MOTOR);
                     break;
                  case 0x25:
                  case 0x28:
                  case 0x29:
                  case 0x66:
                  case 0x89:
                     LIFT_Data.fMotorFault = 1;
                     NVM_SaveFault(FAULT_PUMP_MOTOR_SHORT);
                     break;
                  case 0x26: // Motor Short Left
                  case 0x31:
                  case 0x36:
//                        case 0x38:
                  case 0x72:
                     NVM_SaveFault(FAULT_LEFT_MOTOR_SHORT);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x27: // Motor Short Right
                  case 0x34:
                  case 0x35: // VMN Not OK Right
                  case 0x39:
//                        case 0x73:
                     NVM_SaveFault(FAULT_RIGHT_MOTOR_SHORT);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x24:
                     NVM_SaveFault(FAULT_MOTOR_POST_SHORTED);
                     LIFT_Data.fMotorFault = 1;
                     break;
                  case 0x01:
                  case 0x10:
                     LIFT_Data.fMotorController = 0;
                     break;
               }
            }
         }
         else if(LIFT_Data.wMotorLEDCounter1 > MOTOR_LED_NIBBLE_OFF_TIME)
         {
            LIFT_Data.fMotorNibble1 = 1;
         }
         else if(LIFT_Data.wMotorLEDCounter1 > MOTOR_LED_MIN_OFF_TIME)
         {
            LIFT_Data.fMotorValidBit1 = 1;
         }

      }

   }
}

/*******************************************************************************
 *
 * PROCEDURE:  ALGOR_Lift()
 *
 * DESCRIPTION: The main control procedure. It is invoked once per control cycle.
 *              to calculate motor  and coils control values.
 *              The actual vlues intitated by main loop.
 *              The control inputs are acepted from chassis switches or platform
 *              controller. The chassis if activated has high priority than platform.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 * LIFT_Data       R/W owner
 * lift_setup      R   nvm data
 ******************************************************************************/

void
ALGOR_Lift(void)
{
   UINT8 fEnable;
   static UINT8 bDirection;//, bLastLevel; // Direction 1= FWD  0=REV
   /*
    * clear control state
    */
   LIFT_Data.calc_fet  = 0;
   LIFT_Data.calc_pwm1 = 0;
   LIFT_Data.calc_pwm2 = 0;
   fEnable = 0;

   // update enable safety state
    if(lift_options.fSensorType != 1)
   {
      LIFT_Data.fEnableSafety = 1;
   }
   else if(LIFT_Data.fEnableSafety == 0)
   {
      if(!SW_ENABLE || (!FOOT_PLATE && lift_options.fFootPlate))
      {
         LIFT_Data.fEnableSafety = 1;
      }
   }
   else
   {
      if(((SW_ENABLE && !LIFT_Data.fEnable) || (FOOT_PLATE && !LIFT_Data.fFootPlate && lift_options.fFootPlate)) &&
         ((PLATFORM_Data.nJoystickCounts > 20) || (PLATFORM_Data.nJoystickCounts < -20)))
      {
         LIFT_Data.fEnableSafety = 0;
      }
   }

   /*
    * update platfrom state
    */

   /*
    * set the movement mode lift or drive
    * change the chassis to platfrom mode only if joystick enable is off
    */
   if(!SW_ENABLE)
   {
      LIFT_Data.fLiftDrive = LIFT_DRIVE;
      NVM_SaveLiftDrive(LIFT_Data.fLiftDrive);
      LIFT_Data.fKeySwitch = KEY_SWITCH;
   }
   // start cnt when enable pressed and joystick not move
    if(SW_ENABLE && ((LIFT_Data.nRampedCounts1 == 0) && !SW_LEFT && !SW_RIGHT) && (u16PcuNoActionCnt > 0))
   {
      u16PcuNoActionCnt--;
   }
   // recover cnt when (enable pressed and joystick move) or (enable released and joystick not move)
    if(SW_ENABLE && ((LIFT_Data.nRampedCounts1 != 0) || SW_LEFT || SW_RIGHT) && (u16PcuNoActionCnt > 0))
   {
      // re-copy count to 10s
      u16PcuNoActionCnt = PCU_NO_ACTION_DELAY;
   }
    if(!SW_ENABLE && ((LIFT_Data.nRampedCounts1 == 0) && !SW_LEFT && !SW_RIGHT))
   {
      // re-copy count to 10s
      u16PcuNoActionCnt = PCU_NO_ACTION_DELAY;
   }
   /*
    *  disable platfrom if lift/drive mode is diferent in platfrom and ecu
    */
   //    else if (  LIFT_DRIVE != LIFT_Data.fLiftDrive)
   //    {
   //        LIFT_Data.fPlatformModeFault = 1;
   //        NVM_SaveFault( FAULT_PLATFORM_MISMATCH_MODE);
   //    }

   /*
    * Change from/to high torque mode any time.
    */
   if(!LIFT_Data.fHigh && SW_HIGH && !LIFT_Data.fLiftDrive)
   {
      LIFT_Data.fHighMode = !LIFT_Data.fHighMode;
   }
   if(BATTERY_Data.fLowVoltageLimpMode)
   {
	  LIFT_Data.fHighMode = 1;
   }
   /*
    * Disable lift operation for time MODE_CHANGE_TIMER
    *      - when control input is switched from/to chassis/platform
    *      - when re-enabling the platform operation,
    */
   if((SCAN_ON_KEY_SWITCH || SCAN_OFF_KEY_SWITCH)
      || (!SW_ENABLE && LIFT_Data.fEnable && KEY_SWITCH))
   {
      LIFT_Data.bModeChangeTimer = MODE_CHANGE_TIMER;
   }
   else if(LIFT_Data.bModeChangeTimer)
   {
      LIFT_Data.bModeChangeTimer--;
   }
   /*
    * disable lift operation if
    *      - overload fault
    *      - no comunication to platfrom
    *      - battery fault ( battery below 9Volt)
    */
   else if(!xfer.fPlatformInit
           //         || (LIFT_Data.fOverload && !DOWN_LIMIT)
           || (LIFT_Data.fOverload && !MAIN_Data.fOverloadLimit)
           //              || MAIN_Data.fAngleFault
           //              || MAIN_Data.fPressureFault
           || SLEEP_Data.fSleep
           || BATTERY_Data.fLowBatteryFault
           || JOYSTICK_Data.fNeutralFault
           || ((lift_options.fOverload2_cutout) && (TIMER_Data.wSystemTime < 6500)))
   {
      LIFT_Data.calc_fet &= ~FET_MCE;  //GTF Turn motor controller off when driving is disabled
      if(ELEC1 == lift_setup.bLiftType  /*|| ELEC2 == lift_setup.bLiftType */
         || ELEC3 == lift_setup.bLiftType)
      {
         LIFT_Data.calc_fet &= ~FET_MCE2;  //GTF Turn motor controller off when driving is disabled
      }
      /*
       * no action lift is disabled
       */
   }
   /*
    * Chassis Key Switch points to chassis
    */
   else if(!KEY_SWITCH && !MAIN_Data.fChassisFault && (CHASSIS_UP_SW || CHASSIS_DOWN_SW))
   {
      /*
       * Lifting up is prohibited if pothole stuck retracted
       * and lift is rised above lower limit.
       * or lift is above upper limit.
       */
      LIFT_Data.calc_fet &= ~FET_MCE;  //GTF Turn motor controller off when lifting
      if((CHASSIS_UP_SW && !CHASSIS_DOWN_SW)
	  	 && !BATTERY_Data.fLowVoltageLimpMode
         && !LIFT_Data.fLiftDriveDisable
         //          && (MAIN_Data.fPotholeTest || POTHOLE_LIMIT)
         //                && !(LIFT_DRIVE_CUTOUT_OPTION
         //                && !DOWN_LIMIT && !LEVEL_SENSOR )
         && MAIN_Data.fUpLimit       //UP_LIMIT
            && !MAIN_Data.fUpDisable
            && !MAIN_Data.bSemiLockMachine
            && !MAIN_Data.bPassiveLockMachine
            && !MAIN_Data.bLockLiftState
            && !xfer.fGpsLostCommunication
            && !MAIN_Data.bOutDoorLSFault
            && !MAIN_Data.bOutDoorModeFault)
      {
         LIFT_Data.calc_pwm1 = CHASSIS_UP_PWM;

         LIFT_UP_ON();
         fEnable = 1;
      }
      //        else if ( !CHASSIS_UP_SW && CHASSIS_DOWN_SW && !(!DOWN_LIMIT && LIFT_Data.fOverload))
      else if(!CHASSIS_UP_SW && CHASSIS_DOWN_SW && !(!MAIN_Data.fOverloadLimit  && LIFT_Data.fOverload))
      {
         LIFT_Data.calc_fet = FET_DOWN /*| FET_BEACON*/; // set lift down
         fEnable = 1;
      }
   }

   /*
    * Chassis key switch points to platform.
    * process the platform.
    */
   else if(KEY_SWITCH)
   {
      /*
       * disbale platform:
       *  if joystick enable (deadman) switch is off
       *  if switched to platfrom when interlock is on
       *  if platfrom driver failure
       *  if platfrom mode mismatch
       *  if joystick enbale fault
       */
      if(
         (!LIFT_Data.fKeySwitch)
         || JOYSTICK_FAILURE == JOYSTICK_Data.eState
         || LIFT_Data.fPlatformModeFault
         || JOYSTICK_Data.fEnableSwitchFault
//         || ROCKER_Data.fEnableSwitchFault
        )
      {
         if(lift_setup.bLiftType == ELEC3)
         {
            LIFT_Data.calc_fet &= ~FET_MCE2;  //GTF Turn motor controller off when faulted
         }
         /*
          * no action platfrom is disabled
          */
      }
      else if (!SW_ENABLE ||
            (FOOT_PLATE && !lift_options.fFootPlate) ||
            (!FOOT_PLATE && lift_options.fFootPlate) ||
            !LIFT_Data.fEnableSafety ||
            ((u16PcuNoActionCnt == 0)&& lift_options.fUS_option))
      {
         if((lift_setup.bLiftType != ELEC3) || LIFT_Data.fLiftDrive)
         {
            LIFT_Data.calc_fet &= ~FET_MCE;  //GTF Turn motor controller off when in lift mode
         }
         /*
          * no action platfrom is disabled
          */
      }
      /*
       * Joystick intrlock (deadman) switch is closed
       * movevement is initiated by joystick count and
       * platform mode.
       */
      else if(LIFT_Data.fLiftDrive)//
      {
         //GTF 3-25-11          if (lift_setup.bLiftType == ELEC3)
         //GTF 3-25-11        {
         //GTF 3-25-11           LIFT_Data.calc_fet &= ~FET_MCE2;  //GTF Turn motor controller off when lifting
         //GTF 3-25-11        }
         LIFT_Data.calc_fet &= ~FET_MCE;  //GTF Turn motor controller off when lifting
         LIFT_Data.pDriveCurve = &lift_up_curve;
         //            LIFT_Data.pSteerCurve =  &steer_fast_curve;
         //       if (lift_options.fType == LIFT_TYPE_MINI)
         //              ramp_up2( );
         //       else
         ramp_up1();
         /*
          * Lifting up is prohibited above upper limit or
          * lower limit when lift is tilted;
          */
         if(!((LIFT_Data.fLiftDriveDisable
               //                (LIFT_DRIVE_CUTOUT_OPTION
               //                         && !DOWN_LIMIT
               //                 && !LEVEL_SENSOR )
               //                 || (!MAIN_Data.fPotholeTest && !POTHOLE_LIMIT )
               //                 || (!DOWN_LIMIT && LIFT_Data.fOverload)
               || (!MAIN_Data.fOverloadLimit && LIFT_Data.fOverload)
                   || !MAIN_Data.fUpLimit/*UP_LIMIT*/ || MAIN_Data.fUpDisable
                   || MAIN_Data.bSemiLockMachine
                   || MAIN_Data.bPassiveLockMachine
                   || MAIN_Data.bLockLiftState
                   || xfer.fGpsLostCommunication
                   || BATTERY_Data.fLowVoltageLimpMode
                   || MAIN_Data.bOutDoorLSFault
                   || MAIN_Data.bOutDoorModeFault
                   || MAIN_Data.bLiftUpPressLiftLimit)
              && (LIFT_Data.nRampedCounts1 > 0)))
         {
            if(LIFT_Data.nRampedCounts1 > 0)
            {
               LIFT_Data.calc_pwm1 = CURVE_GetPwm(LIFT_Data.nRampedCounts1,
                                                  LIFT_Data.pDriveCurve);

               LIFT_UP_ON();
               fEnable = 1;
            }
            else if(LIFT_Data.nRampedCounts1 < 0)     //GTF 5-9-09  was 0
            {
               // add dead-zone for US machine
               if(!lift_options.fUS_option || (lift_options.fUS_option && (LIFT_Data.nRampedCounts1 < -30)))
               {
                  LIFT_Data.calc_pwm1 = 0;
                  LIFT_Data.calc_fet = FET_DOWN; // set lift down
               }
               fEnable = 1;
            }
            else
            {
               LIFT_Data.calc_fet &= ~FET_MCE & ~FET_MCE2;  //GTF Turn motor controller off when driving is disabled
               fEnable = 0;
            }
//            fEnable = 1;

         }
         else
         {
            LIFT_Data.calc_fet &= ~FET_MCE & ~FET_MCE2;  //GTF Turn motor controller off when driving is disabled
         }
      }
      else if(((!MAIN_Data.bOutDoorMode && lift_options.fKorea_option) || !lift_options.fKorea_option)
	  		 
              && !MAIN_Data.bOutDoorLSFault
              && !MAIN_Data.bSecondLockLift
              && !MAIN_Data.bOutDoorModeFault)
      {
         /*
          * Driving is disabled if lift is above upper safety guard,
          * lift tilted or pothole is still retracted and lift isn't stowed.
          */
         if(
		 	(!LIFT_Data.fLiftDriveDisable)
		  	&&(!(lift_options.fKorea_option&&CRASH_LIMIT))
		  )
            //          ( (LIFT_DRIVE_CUTOUT_OPTION
            //                       && !DOWN_LIMIT && !LEVEL_SENSOR )
            //                   || (!MAIN_Data.fPotholeTest && !POTHOLE_LIMIT && !DOWN_LIMIT)
            //             || !FREEWHEEL_SW) )
         {
            //            if (lift_setup.bLiftType == ELEC3 && LIFT_Data.wNeutralBrakeApplyTimer)
            //          {
            //             LIFT_Data.calc_fet |= FET_MCE;  //GTF Turn motor controller on when driving mode
            //          }
            /*
             * Drive very slowly ( rised mode ) if
             * pothole is extracted (not Korea machine),
             * or above down limit (Korea machine),
             * or joystick is broken,
             * or battery is almost disharged.
             */
            if (!POTHOLE_LIMIT && !JOYSTICK_NEUTRAL_FAULT)// && !BATTERY_Data.fLowVoltageLimpMode
            {
               if(LIFT_Data.fHighMode)
               {
                  LIFT_Data.pDriveCurve = &drive_slow_curve;
               }
               else
               {
                  LIFT_Data.pDriveCurve = &drive_fast_curve;
                  //LIFT_Data.pSteerCurve = &steer_fast_curve;
               }
            }
            else
            {
               /*
                * Always in high torque in rised mode.
                */
               LIFT_Data.fHighMode = 1;
               LIFT_Data.pDriveCurve =  &drive_rised_curve;
               //                    LIFT_Data.pSteerCurve =  &steer_rised_curve;
            }

            if(CURVE_IsEnabled(LIFT_Data.pDriveCurve))
            {
               ramp_up1();
               DRIVE_PwmAndFet1();
               // <40_GARY_ENABLE_FIX>
               //             GTFtemp=LIFT_Data.nRampedCounts1;
               if((ELEC3 != lift_setup.bLiftType) || SW_RIGHT || SW_LEFT || LIFT_Data.nRampedCounts1_X_1000)
                  // <40_GARY_ENABLE_FIX>
                  fEnable = 1;
            }
         }
         else
         {
            if (ELEC1 == lift_setup.bLiftType  )   LIFT_Data.calc_fet &= ~FET_MCE2; //GTF Turn motor controller off when driving is disabled
            else                          LIFT_Data.calc_fet &= ~FET_MCE;  //GTF Turn motor controller off when driving is disabled
         }
      }
   }
   /*
    * Chassis Key Switch points to chassis
    */
   else if(!KEY_SWITCH && !MAIN_Data.fChassisFault)
   {
      /*
       * Lifting up is prohibited if pothole stuck retracted
       * and lift is rised above lower limit.
       * or lift is above upper limit.
       */
      LIFT_Data.calc_fet &= ~FET_MCE;  //GTF Turn motor controller off when driving disabled
      if((CHASSIS_UP_SW && !CHASSIS_DOWN_SW)
         && !LIFT_Data.fLiftDriveDisable
         //           && (MAIN_Data.fPotholeTest || POTHOLE_LIMIT)
         //                 && !(LIFT_DRIVE_CUTOUT_OPTION
         //              && !DOWN_LIMIT && !LEVEL_SENSOR )
         && MAIN_Data.fUpLimit
       && !MAIN_Data.bOutDoorLSFault
         && !MAIN_Data.fUpDisable
         && !MAIN_Data.bOutDoorModeFault)
      {
         LIFT_Data.calc_pwm1 = 0;
         LIFT_UP_ON();
         fEnable = 1;
      }
      //        else if ( !CHASSIS_UP_SW && CHASSIS_DOWN_SW && !(!DOWN_LIMIT && LIFT_Data.fOverload))
      else if(!CHASSIS_UP_SW && CHASSIS_DOWN_SW && !(!MAIN_Data.fOverloadLimit && LIFT_Data.fOverload))
      {
         LIFT_Data.calc_fet = FET_DOWN ; // set lift down
         fEnable = 1;
      }
   }

   /*
    * save platform state for the next cycle
    */
   LIFT_Data.fHigh = SW_HIGH;
   LIFT_Data.fEnable = SW_ENABLE;
   LIFT_Data.fFootPlate = FOOT_PLATE;

   /*
    * set 4 seconds safety timer and inerlock release flag
    * for transition below downlimit.
    */
   if(!MAIN_Data.fDescentLimit)
   {
      LIFT_Data.fDownSafetyCheck = 1;
      LIFT_Data.wDownSafetyTimer = CONTROL_INTERVAL_MS(4000); // four second
   }
   else
   {
      if(LIFT_Data.wDownSafetyTimer)
      {
         LIFT_Data.wDownSafetyTimer--;
      }
      //
      // Allow desent down when iterlock is released.
      //
      if(!SW_ENABLE && !CHASSIS_DOWN_SW)
      {
         LIFT_Data.fDownSafetyCheck = 0;
      }
   }
   /*
    * set 2 seconds safety timer for any descent in stowed mode.
    */
   if(!(LIFT_Data.calc_fet & FET_DOWN))
   {
      LIFT_Data.wDownSafetyTimerStowed = CONTROL_INTERVAL_MS(2000);
   }
   else if(LIFT_Data.wDownSafetyTimerStowed)
   {
      LIFT_Data.wDownSafetyTimerStowed--;
   }

   /*
    * set 2 seconds descent delay timer for US machine
    */
   if(!(LIFT_Data.calc_fet & FET_DOWN) && (LIFT_Data.hd_fet & FET_DOWN))
   {
      LIFT_Data.wUSDownDelayTimer = CONTROL_INTERVAL_MS(2000);
   }
   else if(LIFT_Data.wUSDownDelayTimer)
   {
      LIFT_Data.wUSDownDelayTimer--;
   }

   /*
    * The heavy machines has significant inertia.
    * Apply a delay while switching from reverse to/from forward
    * driving.
    */
   if((LIFT_Data.hd_fet & FET_REVERSE)/* && (LIFT_Data.nRampedCounts1< 0)*/)
   {
      LIFT_Data.wForwardSwitchTimer = DRIVE_DIRECTION_SWITCH_DELAY;
      bDirection = 0; // 1= FWD  0=REV
   }
   else if(LIFT_Data.wForwardSwitchTimer)
   {
      LIFT_Data.wForwardSwitchTimer--;
   }


   if((LIFT_Data.hd_fet & FET_FORWARD) /*&& (LIFT_Data.nRampedCounts1 >0)*/)
   {
      LIFT_Data.wReverseSwitchTimer = DRIVE_DIRECTION_SWITCH_DELAY;
      bDirection = 1; // 1= FWD  0=REV
   }
   else if(LIFT_Data.wReverseSwitchTimer)
   {
      LIFT_Data.wReverseSwitchTimer--;
   }

   /*
    * reset motor on timer if motor disabled
    */
   if(!(LIFT_Data.hd_fet & FET_MCE))
   {
      LIFT_Data.bMotorOnTimer = MOTOR_ON_DELAY;
   }
   else if(LIFT_Data.bMotorOnTimer)
   {
      LIFT_Data.bMotorOnTimer--;
   }

   if(!(LIFT_Data.hd_fet & FET_MCE2))
   {
      LIFT_Data.bMotorOnTimer2 = MOTOR_ON_DELAY;
   }
   else if(LIFT_Data.bMotorOnTimer2)
   {
      LIFT_Data.bMotorOnTimer2--;
   }
   // ET_WORKING <BRAKE_SYNC_ZERO_DETECT>
   if(ELEC1 == lift_setup.bLiftType)
   {
      if(LIFT_Data.bLevelBrakeApplyTimer && MOTOR_CONTROL_INPUT2)
         LIFT_Data.bLevelBrakeApplyTimer--;
   }
   else
   {
      if(LIFT_Data.bLevelBrakeApplyTimer)
         LIFT_Data.bLevelBrakeApplyTimer--;
   }

   if((LIFT_Data.calc_fet & (FET_FORWARD | FET_REVERSE)) && !LIFT_Data.fLiftDrive)     // Driving
   {
      // ET_WORKING <BRAKE_SYNC_ZERO_DETECT>
      if(ELEC3 == lift_setup.bLiftType)
      {
         if(((LIFT_Data.nRampedCounts1 < 0) && !bDirection) ||
            ((LIFT_Data.nRampedCounts1 > 0) && bDirection))
            LIFT_Data.bLevelBrakeApplyTimer = LEVEL_BRAKE_APPLY_DELAY;
         if(!((LIFT_Data.wNeutralBrakeApplyTimer < 10) && (!SW_ENABLE)))
            LIFT_Data.wNeutralBrakeApplyTimer = NEUTRAL_BRAKE_APPLY_DELAY;
      }
      else
      {
         LIFT_Data.bLevelBrakeApplyTimer = LEVEL_BRAKE_APPLY_DELAY;
         LIFT_Data.wNeutralBrakeApplyTimer = NEUTRAL_BRAKE_APPLY_DELAY;
      }
      // ET_WORKING <BRAKE_SYNC_ZERO_DETECT>
   }
   else if((LIFT_Data.calc_fet & (FET_FORWARD | FET_REVERSE)) && LIFT_Data.fLiftDrive)   // Lifting
   {
      LIFT_Data.wNeutralBrakeApplyTimer = NEUTRAL_BRAKE_APPLY_DELAY / 4;
   }
   else if(LIFT_Data.wNeutralBrakeApplyTimer)
   {
      LIFT_Data.wNeutralBrakeApplyTimer--;
   }
   // <TILT_DELAY>
   //    if (!( LIFT_Data.hd_fet & FET_MCE ) && !LEVEL_SENSOR) // If not moving...
   // {
   //    LIFT_Data.wTiltBrakeApplyTimer = TILT_BRAKE_ON_DELAY;
   // }
   // <TILT_DELAY>

   if(fEnable)
   {
      UINT16 calc_fet;

      calc_fet = LIFT_Data.calc_fet;

      /*
       * Driving delay section
       */

      /*
       * The driving is delayed. The brakes is not applied
       * the lift is moving by inertia.
       */
      if(LIFT_Data.wForwardSwitchTimer && (calc_fet & FET_FORWARD))
      {
         //         if ( ELEC3 == lift_setup.bLiftType && LIFT_Data.wNeutralDirectionApplyTimer)
         //          {
         //                calc_fet &= ~(FET_FORWARD);
         //       }
         /*else*/    if(ELEC1 == lift_setup.bLiftType  /*|| ELEC2 == lift_setup.bLiftType*/
                        || ELEC3 == lift_setup.bLiftType)
         {
            calc_fet &= ~(FET_FORWARD);
         }
         else
         {
            calc_fet &= ~(FET_FORWARD | FET_PARALLEL);
         }

         FORCE_NEUTRAL();
      }
      if(LIFT_Data.wReverseSwitchTimer && (calc_fet & FET_REVERSE))
      {
//       if ( ELEC3 == lift_setup.bLiftType && LIFT_Data.wNeutralDirectionApplyTimer)
//       {
////          calc_fet &= ~(FET_REVERSE);
//       }
         /*else */   if(ELEC1 == lift_setup.bLiftType   /*|| ELEC2 == lift_setup.bLiftType*/
                        || ELEC3 == lift_setup.bLiftType)
         {
            calc_fet &= ~(FET_REVERSE);
         }
         else
         {
            calc_fet &= ~(FET_REVERSE | FET_PARALLEL);
         }
         FORCE_NEUTRAL();
      }

      // lift down safety check section

      // delay down if lift is
      //  - below down limit or going to stowed position
      //  - just turn power on.
      if(DESCENT_DELAY_OPTION
         && MAIN_Data.fDescentLimit
//       && POTHOLE_LIMIT
         && (LIFT_Data.fDownSafetyCheck
             || LIFT_Data.wDownSafetyTimer
             || LIFT_Data.wDownSafetyTimerStowed
             || TIMER_Data.wSystemTime < 3000))
      {
         calc_fet &= ~FET_DOWN;
      }
      if((lift_options.fOverload2_cutout) && TIMER_Data.wSystemTime < 6500)
      {
         calc_fet &= ~FET_DOWN;
      }

      // delay down if US 0807 machine and just lowered
      if((ELEC3 == lift_setup.bLiftType) && lift_options.fUS_option && LIFT_Data.wUSDownDelayTimer)
      {
         calc_fet &= ~FET_DOWN;
      }

      // motor on delay section

      // delay till motor on timer is counted down
      if(LIFT_Data.bMotorOnTimer && (calc_fet & FET_MCE))
      {
         FORCE_NEUTRAL();
      }

      if(LIFT_Data.bMotorOnTimer2 && (calc_fet & FET_MCE2)
         && (lift_setup.bLiftType == ELEC1 /*|| ELEC2 == lift_setup.bLiftType*/)
        )
      {
         calc_fet &= (~FET_FORWARD & ~FET_REVERSE);
         FORCE_NEUTRAL();
      }
      // 10A = R = 1B-MC  = RIGHT_BRAKE  = SWITCH_Data.wSwitches.sw.b7
      // SHORT IN MOTION
      // If MC drives brake LOW while Driving, stop LIFT and wait for MC Error Message
      if(ELEC3 == lift_setup.bLiftType)
      {
         // <REV_51_ECU_83_ERROR>
         // See note at Zapi Brake Detect function.
         // Testing HIGH Side of the ZAPI BRAKE.
         if(!RIGHT_BRAKE && DebounceMCESleep && (LIFT_Data.calc_pwm2))
            DebounceMCESleep--;
         else
            DebounceMCESleep = 15;

         if(!DebounceMCESleep && !RIGHT_BRAKE  && (LIFT_Data.calc_pwm2))
         {
            NVM_SaveFault(FAULT_RIGHT_BRAKE_COIL);
            LIFT_Data.fMotorFault = 1;
         }
         // Testing LOW Side of the ZAPI BRAKE.
         if(!(calc_fet & (FET_FORWARD | FET_REVERSE)))
         {
            if (Zapi_Brake_Detect( ))  calc_fet &= ~FET_BRAKES;
         }
      }
      //
      // delay brakes if platform is leveled
      // and will not slip on heals
      //
      if(LEVEL_SENSOR
         && (((SMALL_SLAB == lift_setup.bLiftType) && (LIFT_Data.hd_fet & FET_BRAKES))
         || ((ELEC1 == lift_setup.bLiftType /*|| ELEC2 == lift_setup.bLiftType*/) && (LIFT_Data.hd_fet & FET_MCE2))
         || ((ELEC3 == lift_setup.bLiftType) && (LIFT_Data.hd_fet & FET_MCE))  //Aded GTF 10-11-11
            )
         && LIFT_Data.wNeutralBrakeApplyTimer)
      {
         // ET_WORKING <ET_C0_CHANGES>
         if(ELEC1 == lift_setup.bLiftType)
         {
            calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_MCE2 | FET_BRAKES));
            if(MOTOR_CONTROL_INPUT2 && iFieldBrake)
            {
               calc_fet &= ~FET_BRAKES;
               LIFT_Data.calc_pwm2 = 0;
            }
         }
         /*else if (ELEC2 == lift_setup.bLiftType)
         {
            calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_MCE2 ) );
         }*/
         // <40_FWD_REV_DIP_IN DELAY>
         /*          else if (ELEC3 == lift_setup.bLiftType)
                  {
                     if (LIFT_Data.bLevelBrakeApplyTimer)
                     {
                        if (bDirection)
                           calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_BRAKES | FET_FORWARD ) );
                        else
                           calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_BRAKES | FET_REVERSE ) );
                     }
                     else
                     {
                        calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_BRAKES ) );
                     }
                     if (!(calc_fet & (FET_FORWARD | FET_REVERSE) ))
                     {
                        if (Zapi_Brake_Detect( ))  calc_fet &= ~FET_BRAKES;
                     }
                  }
         */
         // <40_FWD_REV_DIP_IN DELAY>
         // ET_WORKING <ET_C0_CHANGES>
         else
         {
            calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_BRAKES)) ;
         }
      }
      // <START> on Incline
      // <TILT_DELAY>
      //         else if ( !LEVEL_SENSOR && (ELEC3 == lift_setup.bLiftType) && (calc_fet & FET_MCE))
      //    {
      //       if (LIFT_Data.wTiltBrakeApplyTimer)
      //       {
      //          LIFT_Data.wTiltBrakeApplyTimer--;
      //          if (!(LIFT_Data.hd_fet & FET_BRAKES))
      //             calc_fet &= ~FET_BRAKES;
      //       }
      //    }
      // <TILT_DELAY>
      // <STOP> on Incline
      else if(!LEVEL_SENSOR && (ELEC1 == lift_setup.bLiftType))
      {
         calc_fet |= (LIFT_Data.hd_fet &  FET_BRAKES);
         if(MOTOR_CONTROL_INPUT2 && iFieldBrake)
         {
            calc_fet &= ~FET_BRAKES;
            LIFT_Data.calc_pwm2 = 0;
         }

      }
      if(ELEC3 == lift_setup.bLiftType && LIFT_Data.wNeutralBrakeApplyTimer)
      {
         if(LIFT_Data.bLevelBrakeApplyTimer)
         {
            if(bDirection)
               calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_BRAKES | FET_FORWARD));
            else
               calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_BRAKES | FET_REVERSE));
         }
         else
         {
            calc_fet |= (LIFT_Data.hd_fet & (FET_MCE | FET_BRAKES));
         }
         if(!(calc_fet & (FET_FORWARD | FET_REVERSE)))
         {
            if (Zapi_Brake_Detect( ))  calc_fet &= ~FET_BRAKES;
         }
      }
      LIFT_Data.hd_fet = calc_fet;
   }
   else
   {
      /*
       * Lift is disabled.
       */
      LIFT_Data.calc_fet = 0;
      if(/*LEVEL_SENSOR &&*/ (ELEC1 == lift_setup.bLiftType /*|| ELEC2 == lift_setup.bLiftType*/))
      {
         if(LIFT_Data.wNeutralBrakeApplyTimer)
         {  // ET_WORKING <ET_C0_CHANGES>
            if(MOTOR_CONTROL_INPUT2 && iFieldBrake && !LIFT_Data.bLevelBrakeApplyTimer)
            {
               LIFT_Data.calc_fet = 0;
            }
            else
            {
               LIFT_Data.calc_fet = FET_BRAKES;
            }                                         // ET_WORKING <ET_C0_CHANGES>
         }
         else
         {
            LIFT_Data.calc_fet = 0;
         }
      }
      // STOP LEVEL OR RAMP
      //ET_WORKING <C4_FWD_BLIP>
      // <C6_MC_DELAY>
      // 100 mSec After SPEED or DELAY-10
      // NOTE: The if above (LIFT_Data.wNeutralBrakeApplyTimer) puts MCE in...
      // if we drop it early we won't have it for backup/redundancy brake at 3,000
      // we can't remove it we need it so we need more bytes we are at 0x7F7F with the following 2 lines.
      //       if ((LIFT_Data.bLevelBrakeApplyTimer > (LEVEL_BRAKE_APPLY_DELAY - 10)) && !LEVEL_SENSOR )
      //          LIFT_Data.calc_fet |= FET_MCE;
      // <C6_MC_DELAY>
      // <BRAKE_SYNC_ZERO_DETECT>
      else if((ELEC3 == lift_setup.bLiftType))
      {

         if((LIFT_Data.wNeutralBrakeApplyTimer < 10) && !LIFT_Data.fMotorFault)
         {
            if ( bDirection )    LIFT_Data.calc_fet = FET_FORWARD;
            else LIFT_Data.calc_fet = FET_REVERSE;
         }
         else LIFT_Data.calc_fet = 0;
         //       if ( LIFT_Data.wNeutralBrakeApplyTimer == 0 )      LIFT_Data.calc_fet = 0;
         if(LIFT_Data.wNeutralBrakeApplyTimer && !LIFT_Data.fMotorFault)
         {
            LIFT_Data.calc_fet |= FET_BRAKES | FET_MCE;
         }
         else
         {
            LIFT_Data.calc_fet = 0;
            LIFT_Data.wNeutralBrakeApplyTimer = 0;
            LIFT_Data.bLevelBrakeApplyTimer = 0;
         }
         if(LIFT_Data.bLevelBrakeApplyTimer && LEVEL_SENSOR)      // <C6_NO_FWD_DELAY_ON_RAMP>
         {
            if(bDirection)    // 1= FWD  0=REV
               LIFT_Data.calc_fet |= FET_FORWARD;
            else
               LIFT_Data.calc_fet |= FET_REVERSE;
         }
         if (Zapi_Brake_Detect( ))  LIFT_Data.calc_fet = 0;
      }
      // ET_WORKING <BRAKE_SYNC_ZERO_DETECT>

      LIFT_Data.hd_fet   = LIFT_Data.calc_fet;
      LIFT_Data.nRampedCounts1_X_1000 = 0;
      LIFT_Data.nRampedCounts2_X_1000 = 0;
      LIFT_Data.calc_pwm1 = 0;
      LIFT_Data.calc_pwm2 = 0;
      LIFT_Data.wNeutralWheelPwmOffTimer = 0;

   }
   iFieldBrake = MOTOR_CONTROL_INPUT2;                            // Debounce
}

void
LIFT_UP_ON(void)
{
   LIFT_Data.calc_pwm2 = 0;
   if(lift_setup.bLiftType == ELEC3)
   {
      LIFT_Data.calc_fet = (FET_UP  | FET_MCE | FET_MCE2);
   }
   else
   {
      LIFT_Data.calc_fet = (FET_UP | FET_MCE);
   }
}

static UINT8
Zapi_Brake_Detect(void)
{
   // This function applies the ECU 0807 brakes if ZAPI applies its brakes while in motion.
   // BRAKE APPLIED
   //    CONDITION IN MOTION     CONDITION STANDBY (this function is not called)
   //          Low side is High        Low side is Low
   //          High side is High       High side is Low
   // BRAKE RELEASED
   //       CONDITION IN MOTION
   //       Low side is Low/PWM'ed
   //          High side is High
   UINT16 wLoop;
   UINT16 wCount;
   UINT8 bBrake;

   wCount = 0;
   wLoop = 0;
   bBrake = 0;
   while(wLoop < 450)                  // Adjusted here for 1100 uSec. To adjust (wCount > 450) & fet update.
   {  // 450 "while" loops = 1100uSec or 10% bigger than the PWM Cycle.
      wLoop++;                   // TESTING LOW SIDE
      if(!u8GetStatus10C())  wCount++;   // 10C = R = 5B-MC  = BRAKE_ON     = SWITCH_Data.wSwitches.sw.b1
      if (wCount > 10)        break;
   }
   if(wCount < 5)
   {
      if(iFieldBrake2 > 2)
         bBrake = 1;   // Remove Enable drops Emergency Brakes.
      else iFieldBrake2++;
   }
   else
   {
      iFieldBrake2 = 0;
   }
   return bBrake;
}
/*******************************************************************************
 *
 * PROCEDURE:  LIFT_setfets()
 *
 * DESCRIPTION: Sets the alarm, horn, and coil fets.
 *              NOTE: The actual fets are updated in ISR time every one millisecond.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

void
LIFT_setfets(void)
{
   static unsigned char LastMCE;
   UINT16 wFets;

   wFets = LIFT_Data.hd_fet;
   if(ELEC3 == lift_setup.bLiftType)
   {
//    if (BRAKE_ON && (MAIN_Data.eState != MAIN_BRAKE_RELEASE) ) // if not in brake release mode
//    {
//        wFets = wFets & ~FET_MCE;
//    }
//    else
//    {
//       wFets |= FET_MCE;
//    }
      if(!LastMCE && (FET_MCE & wFets))
      {
         wFets = wFets & ~FET_MCE2 & ~FET_FORWARD & ~FET_REVERSE;
      }
      LastMCE = FET_MCE & wFets;
   }
   if(SW_HORN && MAIN_Data.eState != MAIN_TRIP_ANGLE_SAVE)
   {
      wFets |= FET_HORN | FET_ALARM;
   }
   if(ALARM_Data.fGround)
   {
      wFets |= FET_ALARM;
   }
   if(LED_Data.fGround)
   {
      wFets |= FET_SPARE;
   }
   if(SLEEP_Data.fSleep)
      wFets = 0;

   INTERRUPTS_DISABLE;
   /*selA, selB, selC,selD Rick@20151111, VerA does not need, cause sel1,2,3 were used for chip selection
   and selA,B,C,D are used for iSensor chenal selection, here just make sure CS_DIS = 0*/
   FETS = (wFets | (FETS & 0xF000));  //sel1, sel2, sel3 Rick@20151106 change 0x7000 to 0xF000
   INTERRUPTS_ENABLE;
   LIFT_Pwm();
}
/*******************************************************************************
 *
 * PROCEDURE:  GET_target_counts()
 *
 * DESCRIPTION: return joystick or rocker counts per platform
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/
static INT8
GET_target_counts(void)
{
   //for US machine, invert joystick direction of up and down
   if(LIFT_Data.fLiftDrive && lift_options.fUS_option)
   {
      return (0 - Y_AXIS_COUNTS);
   }
   else
   {
      return Y_AXIS_COUNTS;
   }
}

/*******************************************************************************
 *
 * PROCEDURE:  GET_steer_counts()
 *
 * DESCRIPTION: return joystick or rocker counts per platform
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/
//static INT8
//GET_steer_counts( void )
//{
//    return X_AXIS_COUNTS;
//}

/*******************************************************************************
 *
 * PROCEDURE:  DRIVE_PwmAndFet1()
 *
 * DESCRIPTION: Calcualte motor control voltage and fets for driving in
 *              different modes.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

static void
DRIVE_PwmAndFet1(void)
{
   if(LIFT_Data.nRampedCounts1 || GET_target_counts())
   {
      if(lift_setup.bLiftType == ELEC1 || lift_setup.bLiftType == ELEC3 /*|| ELEC2 == lift_setup.bLiftType*/)
      {

         //       LIFT_Data.calc_pwm2 = CURVE_GetPwm( LIFT_Data.nRampedCounts1, LIFT_Data.pDriveCurve);
         // ET_WORKING <ET_C0_CHANGES>
         if(MOTOR_CONTROL_INPUT2 && iFieldBrake && (lift_setup.bLiftType == ELEC1))
         {
            LIFT_Data.calc_pwm2 = 0;
         }
         else
         {
            LIFT_Data.calc_pwm2 = CURVE_GetPwm(LIFT_Data.nRampedCounts1, LIFT_Data.pDriveCurve);
         }
//end of calling routine         iFieldBrake = MOTOR_CONTROL_INPUT2;
      }
      else
         LIFT_Data.calc_pwm1 = CURVE_GetPwm(LIFT_Data.nRampedCounts1, LIFT_Data.pDriveCurve);



      if((!LIFT_Data.fHighMode)
         && (/*LARGE_SLAB == lift_setup.bLiftType
                 ||*/ SMALL_SLAB == lift_setup.bLiftType))
      {
         LIFT_Data.calc_fet = FET_PARALLEL;
      }

      LIFT_Data.calc_fet |= FET_MCE;
      if(ELEC1 == lift_setup.bLiftType  /*|| ELEC2 == lift_setup.bLiftType*/
         || (ELEC3 == lift_setup.bLiftType
             && (LIFT_Data.calc_fet & (FET_RIGHT | FET_LEFT | FET_UP))))
      {
         LIFT_Data.calc_fet |= FET_MCE2;
      }

      if(LIFT_Data.nRampedCounts1 > 0 /*||
            (ELEC3 == lift_setup.bLiftType && (LIFT_Data.calc_fet & FET_FORWARD)
            && LIFT_Data.wNeutralDirectionApplyTimer)*/)
      {  // forward direction
         LIFT_Data.calc_fet &= ~FET_REVERSE;
         if(ELEC1 == lift_setup.bLiftType)
         {
            LIFT_Data.calc_fet |= (FET_FORWARD | FET_MCE2 | FET_BRAKES);
         }
//           /*else if (ELEC2 == lift_setup.bLiftType )
//          {
//            LIFT_Data.calc_fet |= (FET_FORWARD | FET_MCE2 /*| FET_BRAKES*//*);

//         }*/
         else
         {
            LIFT_Data.calc_fet |= (FET_FORWARD | FET_BRAKES);
         }
      }
      else if(LIFT_Data.nRampedCounts1 < 0 /*||
                (ELEC3 == lift_setup.bLiftType && (LIFT_Data.calc_fet & FET_REVERSE)
                && LIFT_Data.wNeutralDirectionApplyTimer)*/)
      {  // reverse direction
         LIFT_Data.calc_fet &= ~FET_FORWARD;
         if(ELEC1 == lift_setup.bLiftType)
         {
            LIFT_Data.calc_fet |= (FET_REVERSE | FET_MCE2 | FET_BRAKES);
         }
//           /*else if (ELEC2 == lift_setup.bLiftType )
//          {
//             LIFT_Data.calc_fet |= (FET_REVERSE | FET_MCE2 /*| FET_BRAKES*//*);
//            }*/
         else if(ELEC3 == lift_setup.bLiftType)
         {
            LIFT_Data.calc_fet |= (FET_REVERSE | FET_BRAKES);
         }
         else
         {
            LIFT_Data.calc_fet |= (FET_REVERSE | FET_BRAKES);
         }
      }

      if((SW_RIGHT) ^
         (SW_LEFT))
      {
         if((lift_options.fType == LIFT_TYPE_ELEC1) /*|| (lift_options.fType == LIFT_TYPE_ELEC2)*/ || (LIFT_TYPE_ELEC3 == lift_options.fType))
         {
            LIFT_Data.calc_pwm1 = NEUTRAL_WHEEL_PWM;
         }
         else
         {
            LIFT_Data.calc_pwm1 += TURN_BOOST;
         }

         LIFT_Data.calc_fet |= (SW_RIGHT)
                               ? (FET_RIGHT)
                               : (FET_LEFT);
         if(lift_options.fType == LIFT_TYPE_ELEC3)
            LIFT_Data.calc_fet |= FET_MCE2;
      }
      else if(ELEC3 == lift_setup.bLiftType
              && !(LIFT_Data.calc_fet & (FET_RIGHT | FET_LEFT | FET_UP)))
      {
         LIFT_Data.calc_fet &= ~FET_MCE2;
      }

      LIFT_Data.wNeutralWheelPwmOffTimer = 0;
   }
   else if((SW_RIGHT) ^
           (SW_LEFT))
   {
      LIFT_Data.calc_pwm1 = NEUTRAL_WHEEL_PWM;
      LIFT_Data.calc_pwm2 = 0;
      LIFT_Data.wNeutralWheelPwmOffTimer = NEUTRAL_WHEEL_PWM_OFF_DELAY;

      LIFT_Data.calc_fet |= (SW_RIGHT)
                            ? (FET_RIGHT | FET_MCE)
                            : (FET_LEFT | FET_MCE);
      if(ELEC3 == lift_setup.bLiftType)
         LIFT_Data.calc_fet |= FET_MCE2;
   }
   // keep the motor control voltage while rotating wheels
   // in neutral to allow right/left cliking.
   else if(LIFT_Data.wNeutralWheelPwmOffTimer)
   {
      LIFT_Data.calc_fet |= FET_MCE;
      if(ELEC3 == lift_setup.bLiftType)
         LIFT_Data.calc_fet |= FET_MCE2;
      LIFT_Data.calc_pwm1 = NEUTRAL_WHEEL_PWM;
      LIFT_Data.wNeutralWheelPwmOffTimer--;
   }
}


/*******************************************************************************
 *
 * PROCEDURE:  ramp_up1()
 *
 * DESCRIPTION: For positive branch of the curve return
 *              the acelearted/deacelerated joystick counts.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

static void
ramp_up1(void)
{
   INT32 nTarget;
   INT32 nRamp;
   INT16 nFlip;
   INT8  target_counts;

   nFlip = ACCEL_DECEL_FACTOR;
   nRamp = LIFT_Data.nRampedCounts1_X_1000;
   target_counts = GET_target_counts();

   //
   // flip coordinates
   //
   if(nRamp < 0)
   {
      nFlip = -nFlip;
      nRamp = -nRamp;
   }
   else if(!nRamp && target_counts < 0)
   {
      nFlip = -nFlip;
   }

   nTarget = (INT32)nFlip * target_counts;

   if(nRamp < nTarget)
   {
      //        nRamp += LIFT_Data.pDriveCurve->nAccel;
      if(!LEVEL_SENSOR && lift_setup.bLiftType == ELEC3)
      {
         nRamp += (4 * LIFT_Data.pDriveCurve->nAccel);
      }
      else
      {
         nRamp += LIFT_Data.pDriveCurve->nAccel;
      }

      if(nRamp > nTarget)
      {
         nRamp = nTarget;
      }
   }
   else if(nRamp > nTarget)
   {
      if((LEVEL_SENSOR || SW_ENABLE_RAW) && !(LIFT_Data.hd_fet & FET_DOWN))
      {
         nRamp -= LIFT_Data.pDriveCurve->nDecel;
      }
      else
      {
         nRamp -= (LIFT_Data.pDriveCurve->nDecel * 50l);
      }

      if(nRamp < nTarget)
      {
         nRamp = (nTarget > 0) ? nTarget : 0;
      }
   }
   //
   // flip coordinates back
   //
   LIFT_Data.nRampedCounts1_X_1000 = (nFlip < 0) ? -nRamp : nRamp;

   LIFT_Data.nRampedCounts1 = LIFT_Data.nRampedCounts1_X_1000 / ACCEL_DECEL_FACTOR;
}


/*******************************************************************************
 *
 * PROCEDURE:  LIFT_Pwm()
 *
 * DESCRIPTION: Set motor control volatge. It is crated with filtered pwm sisgnal.
 *              Hardware doesn't directly support 0..100% PWM. The PWM timer is stoped
 *              and timer output controlled manualy ofr 0, and 100%.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

//static
void LIFT_Pwm(void)
{
   vSetDuty9A(LIFT_Data.calc_pwm1);
   vSetDuty4C(LIFT_Data.calc_pwm2);
}

#if 0
/*******************************************************************************
 *
 * PROCEDURE:  atanApprox( INT16, INT16 )
 *
 * DESCRIPTION: Retruns approximate arctangent value in degreews * 10
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

INT16
atanApprox(INT16 x, INT16 y)
{
   UINT8 ix;
   INT16 wY, wX;
   INT16 wOffset;
   INT32 nRatio;

   nRatio = ((((INT32)(x) * 450 * 2) + (INT32)(y)) / (INT32)(y)) / 2;
   if(nRatio < 0)
      nRatio = - nRatio;

   for(ix = 1; ix < NUMBER_OF_TABLE_POINTS; ix++)
   {
      if(nRatio <= atanTable[ ix])
      {
         wX = atanTable[ ix] - atanTable[ ix - 1];
         nRatio -= atanTable[ ix - 1];

         wY = atanTable[ ix + NUMBER_OF_TABLE_POINTS] - atanTable[ ix - 1 + NUMBER_OF_TABLE_POINTS];
         wOffset = atanTable[ ix - 1 + NUMBER_OF_TABLE_POINTS];

         if(wX)
         {
            return (wOffset + (wY * (INT16)(nRatio) * 2 + wX) / wX / 2);
         }
         else
         {
            return (wOffset);
         }
      }
   }
   return (atanTable[ NUMBER_OF_TABLE_POINTS - 1 + NUMBER_OF_TABLE_POINTS]);
}

/*******************************************************************************
 *
 * PROCEDURE:  atan2Approx( INT16, INT16 )
 *
 * DESCRIPTION: Retruns approximate arctangent value in degrees * 10
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

INT16
atan2Approx(INT16 x, INT16 y)
{
   INT16 nAngle;
   INT16 nX, nY;

   nX = x;
   nY = y;
   if(nX < 0)
      nX = -nX;
   if(nY < 0)
      nY = -nY;
   if(nX < nY)
   {
      nAngle = atanApprox(nX, nY);
   }
   else
   {
      //    nAngle = 900 - atanApprox(y,x);
      nAngle = atanApprox(nY, nX);
      nAngle = 900 - nAngle;
   }
   if(x < 0)
   {
      if(y < 0)
      {
         nAngle -= 1800;
      }
      else
      {
         nAngle = -nAngle;
      }
   }
   else
   {
      if(y < 0)
      {
         nAngle = 1800 - nAngle;
      }
   }
   return nAngle;
}
#endif

#if 0
/*******************************************************************************
 *
 * PROCEDURE:  GET_magnitude( INT16, INT16 )
 *
 * DESCRIPTION: Retruns magnitude of x and y cartesian values
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/

INT16
GET_magnitude(INT16 x, INT16 y)
{
   INT16 ix;
   INT16 nX, nY, nTemp1, nTemp2;

   if(x < 0)
      nX = -x;
   else
      nX = x;

   if(y < 0)
      nY = -y;
   else
      nY = y;

   if(nX < nY)
   {
      nTemp1 = nX;
      nX = nY;
      nY = nTemp1;
   }
   nTemp2 = nX * nX;
   nTemp1 = nTemp2 + nY * nY;
   for(ix = nX + 1; ix < 180; ix++)
   {
      nTemp2 = nTemp2 + 2 * ix - 1;
      if(nTemp2 > nTemp1)
         return (ix - 1);
   }
   return 179;
}
#endif
/*******************************************************************************
 *
 * PROCEDURE:  InitTimerW()
 *
 * DESCRIPTION: reinit time W in PWM mode.
 *
 * VARIABLE NAME  R/W DESCRIPTION
 * ---------------- --- --------------------------------------------------------
 ******************************************************************************/
/*
static void
InitTimerW( void )
{
    TW.TIOR1.BYTE = 0x03;      // clear output when C match TCNT.
    TW.TIOR0.BYTE = 0x30;//  0x20;   // clear output when B match TCNT.
    TW.TSRW.BYTE  &= 0;      // clear all flags
    TW.TIERW.BYTE = 0;      //disable all interrupts
    TW.TCRW.BYTE = 0x86;    // CCLR and TOB and TOC. TCNT cleared by A and outpur set to 1
                            // when B match TCNT, zero output
                            // when C match TCNT, zero output
                            // CKS2:0 = B'000 - system clcok feeds TCNT
    TW.GRA = PWM_PERIOD;
    //TW.GRD = TW.GRB = 0;    // zero PWM.
                       // start timer, in PWM mode and use D as buffer for B.
//    TW.TMRW.BYTE = ( 0 == f.un_buffered) ? 0xA1 : 0x81; //0xA1;    // CTS, BUFEB, PWMB
    TW.TMRW.BYTE = 0x83;      // un-buffered mode
}
*/




















































































































































